// 1.包含头文件
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/u_int16.hpp"
#include "serial/serial.h"
#include <chrono>
#include <thread>

// 可以直接使用时间单位
using namespace std::chrono_literals;

// 实例化串口对象
serial::Serial ser;

// 3.自定义节点类；
class Mynode : public rclcpp::Node
{
public:
  Mynode() : Node("mynode_node_cpp")
  {
    RCLCPP_INFO(this->get_logger(), "发布方创建！");

    // 创建发布方
    talker_ = this->create_publisher<std_msgs::msg::UInt16>("talker", 10);

    // 创建定时器
    timer_ = this->create_wall_timer(25ms, std::bind(&Mynode::on_timer, this));
  }

private:
  void on_timer()
  {
    // 向编码器发送消息
    uint8_t senddata[8] = {0x01, 0x03, 0x00, 0x00, 0x00, 0x01, 0x84, 0x0A};
    ser.write(senddata, 8);
    // 等待25ms，确保数据可以被接收
    std::this_thread::sleep_for(std::chrono::milliseconds(25));
    // 为接收的数据创造一个空数组
    uint8_t data[7] = {0};
    ser.read(data, 7);

    // // 输出获取的数据
    // for (size_t i = 0; i < 7; ++i)
    // {
    //   RCLCPP_INFO(this->get_logger(), "%02X ", data[i]);
    // }
    // RCLCPP_INFO(this->get_logger(), "\n"); // 添加换行符以结束输出

    // 取出有效数据位 将第三位左移动8位，将第四放在高16位上
    uint16_t combined_value = (data[3] << 8) | data[4];
    // 转换数据格式
    float value = combined_value * 360.0f / 32768.0f;
    // 打印角度
    RCLCPP_INFO(this->get_logger(), "Combined decimal value: %.2f", value);
  }

  rclcpp::Publisher<std_msgs::msg::UInt16>::SharedPtr talker_;
  rclcpp::TimerBase::SharedPtr timer_;
};

int main(int argc, char const *argv[])
{
  // 2.初始化ROS2客户端；
  rclcpp::init(argc, argv);

  // 串口设置
  try
  {
    ser.setPort("/dev/ttyUSB0");                                  // 选择开启的串口
    ser.setBaudrate(9600);                                        // 设置波特率
    serial::Timeout _time = serial::Timeout::simpleTimeout(2000); // 超时等待
    ser.setTimeout(_time);
    ser.open(); // 开启串口
    if (ser.isOpen())
    {
      std::cout << "\n\n ----serial port is open---- \n\n"
                << std::endl;
    }
    else
    {
      std::cout << "serial port error" << std::endl;
    }
  }
  catch (const serial::IOException &e)
  {
    std::cerr << "\n\n----Serial I/O Exception: " << e.what() << "----\n请检查设备是否连接\n是否开放串口权限\n串口是否占用\n"
              << std::endl;
  }

  // ser.setPort("/dev/ttyUSB0");                                  // 选择开启的串口
  // ser.setBaudrate(9600);                                        // 设置波特率 z
  // serial::Timeout _time = serial::Timeout::simpleTimeout(2000); // 超时等待
  // ser.setTimeout(_time);
  // ser.open(); // 开启串口
  // if (ser.isOpen())
  // {
  //   std::cout << "serial port is open" << std::endl;
  // }
  // else
  // {
  //   std::cout << "serial port error" << std::endl;
  // }

  // 4.调用spain函数，并传入节点对象指针；
  rclcpp::spin(std::make_shared<Mynode>());
  // 5.资源释放
  rclcpp::shutdown();
  return 0;
}